#include "windmill.hpp"
#include "ceres/ceres.h"
using namespace std;
using namespace cv;
using namespace ceres;

#define Black Scalar(0,0,0)
#define Red Scalar(0,0,255)
#define Green Scalar(0,255,0)
#define Blue Scalar(255,0,0)

double A=1,phi=1,W=2,b=1;

struct SinCost {
    SinCost(double x, double y):x_(x),y_(y){}
    template <typename T>
    bool operator()(const T* const A,const T* const w,const T* const phi,const  T* const b,T* residual) const {
        residual[0] = y_ - A[0] * sin(w[0] * x_ + phi[0]) - b[0];
        if(w[0]>2.2 || w[0]<1.1)residual[0]+=3;       //由RoboMaster规则手册得出参数的范围
        return true;
    }
    private:
        const double x_;
        const double y_;
};

inline double get_dis(Point2d a,Point2d b){
    return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}

inline double get_angle(Point2f a,Point2f b){
    double dy=a.x-b.x,dx=a.y-b.y;
    double angle=atan(dy/dx);
    if(dx<0 && dy>0)angle+=3.1415926;
    if(dx<0 && dy<0)angle+=3.1415926;
    if(dx>0 && dy<0)angle+=6.2831852;
    return angle;
}

inline double deta_a(double a,double b){
    double ans=abs(a-b);
    if(ans>3.1415926)return 6.2831852-ans;
    return ans;
}

void draw(vector<pair<double,double> > &p,int type){
    Mat g(Size(1500,800),CV_8UC3,Scalar(255,255,255));  //画图
    int X=g.cols-20,Y=g.rows-20;
    Point O=Point(20,Y);
    circle(g,O,3,Black,-1);
    line(g,O,Point(20,20),Black,2);
    line(g,Point(10,30),Point(20,20),Black,2);
    line(g,Point(30,30),Point(20,20),Black,2);
    line(g,O,Point(X,Y),Black,2);
    line(g,Point(X-10,Y-10),Point(X,Y),Black,2);
    line(g,Point(X-10,Y+10),Point(X,Y),Black,2);
    double x_max=p[p.size()-1].first;
    double times=(X-O.x)/x_max;
    if(type)times=type;
    for(int i=0;i<p.size();i++){
        double x=p[i].first,y=p[i].second;
        circle(g,O+Point(x*times,-300*y),3,Blue,-1);
    }
    for(int i=O.x;i<=g.cols-30;i++)
        circle(g,Point(i,O.y-300*(A*sin(W*(i-O.x)/times+phi)+b)),3,Red,-1);
    imshow("graph",g);
}

int main()
{
    //==========================代码区========================//
    std::chrono::milliseconds lst,t,start = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    WINDMILL::WindMill wm(start.count());
    Mat src;

    bool flg=true;
    double angle,lst_angle=0;
    unsigned int tick=0;
    
    vector<pair<double,double> > point;
    Problem problem;
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;
    Solver::Summary summary;
    while (1)
    {
        tick++;
        t = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
        src = wm.getMat((double)t.count()/1000);
        Point2f center,target[5];
        Mat img=src;
        cvtColor(src,img,COLOR_BGR2GRAY);
        threshold(img,img,50,255,THRESH_BINARY);    //二值化
        vector<vector<Point> > contours;
        vector<Vec4i> hierarchy;
        findContours(img,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE); //找轮廓
        int cnt=1;
        for(int i=0;i<contours.size();i++){
            if(hierarchy[i][3]==-1){
                double s=contourArea(contours[i]);
                double c=arcLength(contours[i],1);
                if(s/(c*c)<0.025){
                    RotatedRect rect=minAreaRect(contours[hierarchy[i][2]]);
                    circle(src,rect.center,5,Blue,-1);      //标记打击目标
                    target[0]=rect.center;
                }else if(s/(c*c)>0.03){
                    RotatedRect rect=minAreaRect(contours[hierarchy[i][2]]);
                    target[cnt++]=rect.center;
                }
            }
        }
        center=Point(0,0);
        for(int i=0;i<5;i++)
            center+=target[i];
        center/=5;
        circle(src,center,4,Green,-1);          //锁定圆心
        double r=get_dis(center,target[0]);
        circle(src,center,r,Green,1);           //绘制旋转轨迹

        angle=get_angle(target[0],center);
        if(flg){
            lst=t;
            lst_angle=angle;
            flg=false;
            continue;
        }
        if(tick%10==0){
            double da=deta_a(angle,lst_angle),dt=t.count()-lst.count();
            double y=da*1000/dt;       //计算角速度
            double x=(lst.count()-start.count())/1000.0;
            point.push_back(make_pair(x,y));   //取点
            CostFunction* cost_f = new AutoDiffCostFunction<SinCost,1,1,1,1,1>(new SinCost(x,y));
            problem.AddResidualBlock(cost_f,nullptr,&A,&W,&phi,&b);
            lst=t;
            lst_angle=angle;
            printf("%lf %lf %lf %lf\n",A,W,phi,b);
        }
        if(tick%20==0 && tick>=200){
            Solve(options,&problem,&summary);
            draw(point,0);
        }
        imshow("result",src);
        char key=waitKey(1);
        if(key=='a')break;
    }

    //=======================================================//
}